import java.awt.Color;
import java.io.BufferedReader;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileWriter;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.ArrayList;

import Jama.Matrix;
import chessboard.ChessBoard;
import chessboard.ChessMan;
import chessboard.Field;
import chessboard.Move;
import chessboard.Type;

public class Controller {

	private static String ROB_IP_ADDRESS = "127.0.0.1";
	private static String TRACK_IP_ADDRESS = "localHost";
	private static String LOGIC_IP_ADDRESS = "141.83.19.210";
	private static int ROB_PORT = 5005;
	private static int TRACK_PORT = 5000;
	private static int LOGIC_PORT = 1337;
	private static Matrix mMatrix, nMatrix;
	private static int FILE_INDEX = -1;
	private static double letterStart = 60;
	private static double letterLRun = 57.5;
	private static double letterNRun = 0;
	private static double numberStart = 52;
	private static double numberNRun = 54.5;
	private static double numberLRun = 0;
	private static double zOffset = 0;
	private static double zLRun = 0;
	private static double zNRun = 0;
	private static String[] TESTMOVES1 = new String[] { "a1h1", "h1h8", "h8a8",
			"a8a1" };
	private static String[] TESTMOVES2 = new String[] { "b1b3", "b3b1", "a1a3",
			"a3a1", "d1d3", "d3d1", "c1c3", "c3c1", "e1e3", "e3e1", "a2a4",
			"a4a2" };
	private static String[] TESTMOVES3 = new String[] { "0102", "0304" };
	private static String[] TESTMOVES4 = new String[] { "a7b4", "a8d4", "d101",
			"a2a8q", "g1g3", "f1f3", "e1g1", "c2c4", "c4b5" };

	static boolean offline, save, connect;
	static RobClient robClient;
	static TrackClient trackClientGripper, trackClientChess;
	static ArrayList<Matrix> xMatrices, yMatrices;
	static ChessBoard chessBoard;
	static LogicClient logicClient;

	public static void main(String[] args) {
		offline = true;
		connect = true;
		save = false;
		mMatrix = new Matrix(4, 4);
		nMatrix = new Matrix(4, 4);
		chessBoard = new ChessBoard();
		init();
		chessBoard.output();
		if (!offline || connect) {
			// System.out.println("empty ChessBoard");
			// chessBoard.get(1, 0).setChessMan(new ChessMan(Type.ROOK,
			// Color.black));
			// chessBoard.output();
			while (true) {
				// System.out.println("while");
				if (logicClient.getListSize() != 0 || connect) {
					ArrayList<Move> s = analyzeMove(logicClient.getNextMove());
					// System.out.print(s.size());
					for (Move m : s) {
						System.out.println(m.getStart() + " --> "
								+ m.getDestination());
						applyMove(m);
					}
				} else {

					for (String s : TESTMOVES3) {
						ArrayList<Move> moves = analyzeMove(s);
						wait(4000);
						for (Move m : moves) {
							applyMove(m);
						}
					}
				}
			}
		}
	}

	private static void goTo(Field f) {
		goTo(f, 0, 0, 80);
	}

	private static Matrix getHom(Field f) {
		if (f == null) {
			System.out.println("field == null");
			return null;
		}
		// System.out.println("goTo " + f);
		Matrix m = calculateJointAngles(f);
		m = nMatrix.times(m).times(mMatrix.inverse());
		m.setMatrix(0, 2, 0, 2, Matrix.identity(3, 3));
		return m;
	}

	private static void goTo(Field f, double x, double y, double z) {
		if (f == null) {
			System.out.println("field == null");
			return;
		}
		// System.out.println("goTo " + f);
		Matrix m = calculateJointAngles(f);
		m = nMatrix.times(m).times(mMatrix.inverse());
		m.set(0, 3, m.get(0, 3) + x);
		m.set(1, 3, m.get(1, 3) + y);
		m.set(2, 3, m.get(2, 3) + z);
		m.setMatrix(0, 2, 0, 2, Matrix.identity(3, 3));
		String message = "MovePTPHomRowWise";
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 4; j++) {
				message += " " + m.get(i, j);
			}
		}
		// System.out.println("message: " + message);
		robClient.writeAndRead(message);
	}

	private static void newOffsetter() {
		robClient.writeAndRead("MovePTPJoints 0 -30 30 0 0 0");
		String[] command = null;
		boolean running = true;
		BufferedReader reader = new BufferedReader(new InputStreamReader(
				System.in));
		Field f = null;
		double[] a1Offsets = null;
		double[] h1Offsets = null;
		double[] a8Offsets = null;
		robClient.writeAndRead("GripperGoHome");
		while (running) {
			try {
				command = reader.readLine().split(" ");
				if (command[0].toLowerCase().equals("goto")
						&& command.length > 1) {
					f = chessBoard.get(command[1].charAt(0),
							(int) (command[1].charAt(1) - 48));
					System.out.println("goto " + f);
					goTo(f);
				} else if (command[0].toLowerCase().equals("set")
						&& command.length > 1) {
					f = chessBoard.get(command[1].charAt(0),
							(int) (command[1].charAt(1) - 48));
					goTo(f);
					goTo(f, 0, 0, 0);
					if (command[1].toLowerCase().equals("a1")) {
						System.out.println(letterStart);
						System.out.println(numberStart);
						System.out.println(zOffset);
						command = reader.readLine().split(" ");
						while (!command[0].toLowerCase().equals("done")) {
							a1Offsets = moveGripper(command);
							letterStart += a1Offsets[0];
							numberStart += a1Offsets[1];
							zOffset += a1Offsets[2];
							System.out.println(letterStart);
							System.out.println(numberStart);
							System.out.println(zOffset);
							goTo(f, 0, 0, 0);
							command = reader.readLine().split(" ");
						}
					} else if (command[1].toLowerCase().equals("h1")) {
						command = reader.readLine().split(" ");
						while (!command[0].toLowerCase().equals("done")) {
							h1Offsets = moveGripper(command);
							letterLRun += h1Offsets[0] / 7.0d;
							numberLRun += h1Offsets[1] / 7.0d;
							zLRun += h1Offsets[2] / 7.0d;
							System.out.println(letterLRun);
							System.out.println(numberLRun);
							System.out.println(zLRun);
							goTo(f, 0, 0, 0);
							command = reader.readLine().split(" ");
						}						
					} else if (command[1].toLowerCase().equals("a8")) {
						command = reader.readLine().split(" ");
						while (!command[0].toLowerCase().equals("done")) {
							a8Offsets = moveGripper(command);
							letterNRun += a8Offsets[0] / 7.0d;
							numberNRun += a8Offsets[1] / 7.0d;
							zNRun += a8Offsets[2] / 7.0d;
							System.out.println(letterNRun);
							System.out.println(numberNRun);
							System.out.println(zNRun);
							goTo(f, 0, 0, 0);
							command = reader.readLine().split(" ");
						}
					} else {
						System.out.println("field not supported");
					}
					goTo(f, 0, 0, 80);
				} else if (command[0].toLowerCase().equals("done")) {
					running = false;
				} else if (command[0].toLowerCase().equals("save")) {
					saveOffset();
				} else if (command[0].toLowerCase().equals("load")) {
					loadOffset();
				} else {
					System.out.println("unknown command");
				}
			} catch (IOException e) {
				e.printStackTrace();
			}
		}
	}

	private static double[] moveGripper(String[] command){
		double[] offsets = new double[] { 0, 0, 0 };
			if (command[0].toLowerCase().equals("x") && command.length > 1) {
				offsets[0] += Double.parseDouble(command[1]);
			} else if (command[0].toLowerCase().equals("y")
					&& command.length > 1) {
				offsets[1] += Double.parseDouble(command[1]);
			} else if (command[0].toLowerCase().equals("z")
					&& command.length > 1) {
				offsets[2] += Double.parseDouble(command[1]);
			}
		
		System.out.println("x = " + offsets[0] + ", y = " + offsets[1]
				+ ", z = " + offsets[2]);
		return offsets;
	}

	private static void offsetter() {
		// TODO a1 anfahren, startOffset setzen, h8 anfahren, laufOffset setzen
		String[] command = null;
		double height = 80;
		boolean running = true;
		BufferedReader reader = new BufferedReader(new InputStreamReader(
				System.in));
		Field f = null;

		robClient.writeAndRead("GripperGoHome");

		while (running) {
			System.out
					.println("commands: \n - goto a1 \n - up \n - down \n - letterStart 10 \n - letterRun 10 \n - numberStart 10 \n - numberRun 10 \n - zOffset 10 \n - load \n - save \n - quit");
			try {
				command = reader.readLine().split(" ");
				if (command[0].equals("goto")) {
					f = chessBoard.get(command[1].charAt(0),
							(int) (command[1].charAt(1) - 48));
					goTo(f);
				} else if (command[0].toLowerCase().equals("letterstart")) {
					if (command[1].charAt(0) == '+'
							|| command[1].charAt(0) == '-') {
						letterStart += Double.parseDouble(command[1]);
					} else {
						letterStart = Double.parseDouble(command[1]);
					}
					goTo(f);
				} else if (command[0].toLowerCase().equals("letterrun")) {
					if (command[1].charAt(0) == '+'
							|| command[1].charAt(0) == '-') {
						letterLRun += Double.parseDouble(command[1]);
					} else {
						letterLRun = Double.parseDouble(command[1]);
					}
					goTo(f);
				} else if (command[0].toLowerCase().equals("numberstart")) {
					if (command[1].charAt(0) == '+'
							|| command[1].charAt(0) == '-') {
						numberStart += Double.parseDouble(command[1]);
					} else {
						numberStart = Double.parseDouble(command[1]);
					}
					goTo(f);
				} else if (command[0].toLowerCase().equals("numberrun")) {
					if (command[1].charAt(0) == '+'
							|| command[1].charAt(0) == '-') {
						numberNRun += Double.parseDouble(command[1]);
					} else {
						numberNRun = Double.parseDouble(command[1]);
					}
					goTo(f);
				} else if (command[0].toLowerCase().equals("zoffset")) {
					if (command[1].charAt(0) == '+'
							|| command[1].charAt(0) == '-') {
						zOffset += Double.parseDouble(command[1]);
					} else {
						zOffset = Double.parseDouble(command[1]);
					}
					height = ChessMan.GRABHEIGHT[f.getChessMan().getType()
							.getNumber()]
							+ zOffset;
					goTo(f, 0, 0, height);
				} else if (command[0].toLowerCase().equals("save")) {
					saveOffset();
				} else if (command[0].toLowerCase().equals("load")) {
					loadOffset();
				} else if (command[0].toLowerCase().equals("quit")) {
					running = false;
				} else if (command[0].toLowerCase().equals("down")) {
					System.out.println(f.getChessMan());
					height = ChessMan.GRABHEIGHT[f.getChessMan().getType()
							.getNumber()]
							+ zOffset;
					goTo(f, 0, 0, height);
				} else if (command[0].toLowerCase().equals("up")) {
					height = 80;
					goTo(f, 0, 0, height);
				} else {
					System.out.println("unknown command");
				}
			} catch (IOException e) {
				e.printStackTrace();
			}
			System.out.println("ls,lr,ns,nr = " + letterStart + ","
					+ letterLRun + "," + numberStart + "," + numberNRun);
		}
		chessBoard.init();
	}

	private static void saveOffset() {
		try {
			FileWriter offsetWriter = new FileWriter("offsetValues/offset.txt");
			offsetWriter.append(String.valueOf(numberStart) + " "
					+ String.valueOf(numberNRun) + " "
					+ String.valueOf(numberLRun) + " "
					+ String.valueOf(letterStart) + " "
					+ String.valueOf(letterNRun) + " "
					+ String.valueOf(letterLRun) + " "
					+ String.valueOf(zOffset) + " "
					+ String.valueOf(zNRun) + " "
					+ String.valueOf(zLRun));
			offsetWriter.close();
		} catch (IOException e) {
			e.printStackTrace();
		}

	}

	private static void loadOffset() {
		try {
			BufferedReader offsetReader = new BufferedReader(
					new InputStreamReader(new FileInputStream(
							"offsetValues/offset.txt")));
			String[] offset = offsetReader.readLine().split(" ");
			if(!(offset.length < 9)){
				numberStart = Double.parseDouble(offset[0]);
				numberNRun = Double.parseDouble(offset[1]);
				numberLRun = Double.parseDouble(offset[2]);
				letterStart = Double.parseDouble(offset[3]);
				letterNRun = Double.parseDouble(offset[4]);
				letterLRun = Double.parseDouble(offset[5]);
				zOffset = Double.parseDouble(offset[6]);
				zNRun = Double.parseDouble(offset[7]);
				zLRun = Double.parseDouble(offset[8]);
			} else {
				System.out.println("unable to read file");
			}
		} catch (FileNotFoundException e) {
			e.printStackTrace();
		} catch (IOException e) {
			e.printStackTrace();
		}
	}

	private static Matrix getBoardPosition() {
		Matrix m1 = new Matrix(4, 4);
		Matrix m = new Matrix(4, 4);
		int counter = 0;
		trackClientChess.write("CM_NEXTVALUE");
		String[] answer = trackClientChess.read().split(" ");
		while (!answer[1].equals("y") && counter < 100) {
			trackClientChess.write("CM_NEXTVALUE");
			answer = trackClientChess.read().split(" ");
			counter++;
		}
		while (!answer[1].equals("y")) {
			robClient.write("MovePTPJoints 0 -30 30 0 0 0");
			robClient.read();

			System.out.println("answer[1] = " + answer[1]);
			try {
				Thread.sleep(20);
			} catch (InterruptedException e) {
				e.printStackTrace();
			}

			trackClientChess.write("CM_NEXTVALUE");
			answer = trackClientChess.read().split(" ");
		}
		for (int k = 0; k < 10; k++) {
			for (int i = 0; i < 3; i++) {
				for (int j = 0; j < 4; j++) {
					m1.set(i, j, Double.parseDouble(answer[4 * i + j + 2]));
				}
			}
			trackClientChess.write("CM_NEXTVALUE");
			answer = trackClientChess.read().split(" ");
			m1.set(3, 3, 1);
			m.plusEquals(m1);
		}
		m.timesEquals(0.1d);
		return m;
	}

	private static Field getNextFreeField() {
		Field f = null;
		for (int i = 0; i < 32; i++) {
			if (i < 8) {
				if (chessBoard.getFields()[0][i].getChessMan().getType() == Type.NONE) {
					f = chessBoard.getFields()[0][i];
					break;
				}
			} else if (i < 16) {
				if (chessBoard.getFields()[9][i - 8].getChessMan().getType() == Type.NONE) {
					f = chessBoard.getFields()[9][i - 8];
					break;
				}
			} else if (i < 24) {
				if (chessBoard.getFields()[10][i - 16].getChessMan().getType() == Type.NONE) {
					f = chessBoard.getFields()[10][i - 16];
					break;
				}
			} else {
				if (chessBoard.getFields()[11][i - 24].getChessMan().getType() == Type.NONE) {
					f = chessBoard.getFields()[11][i - 24];
					break;
				}
			}
		}
		System.out.println("next free field: " + f);
		return f;
	}

	private static boolean isReachable(Matrix m) {
		double x = m.get(0, 3);
		double y = m.get(1, 3);
		double z = m.get(2, 3);
		// m.print(0, 4);
		String possible = "IsPossible ";
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 4; j++) {
				possible += String.valueOf(m.get(i, j) + " ");
			}
		}
		possible += "noflip toggleElbow toggleArm";
		robClient.write(possible);
		if (robClient.read().equals("false")) {
			System.out.println("possible = " + possible);
			return false;
		} else if (z < 245 || y < -600 || y > 300 || x < -450) {
			System.out.println("x = " + x + "\ny = " + y + "\nz = " + z);
			return false;
		} else {
			return true;
		}
		// if (Math.sqrt(x * x + y * y + z * z) > 1000) {
		// System.out
		// .println(String.valueOf((Math.sqrt(x * x + y * y + z * z))));
		// return true;
		// } else {
		// // TODO Arbeitsbereich ausmessen
		// return true;
		// }
	}

	private static void wait(int ms) {
		try {
			Thread.sleep(ms);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
	}

	private static Field getTakenChessMan(Type t, Color c) {
		System.out.println("looking for " + t + ", " + c);
		Field highest = null;
		for (int j = 0; j < 3; j++) {
			for (int i = 0; i < 8; i++) {
				System.out.println("i = " + i + ", j = " + j);
				System.out.println("highest = " + highest);
				if (j < 1) {
					if (chessBoard.getFields()[j][i].getChessMan().getType() == t
							&& chessBoard.getFields()[j][i].getChessMan()
									.getColor() == c) {
						System.out.println("ChessMan found at "
								+ chessBoard.getFields()[j][i]);
						return chessBoard.getFields()[j][i];
					} else if (highest != null
							&& highest.getChessMan().getType().getNumber() > chessBoard
									.getFields()[j][i].getChessMan().getType()
									.getNumber()
							&& chessBoard.getFields()[j][i].getChessMan()
									.getType() != Type.NONE
							&& chessBoard.getFields()[j][i].getChessMan()
									.getColor() == c) {
						highest = chessBoard.getFields()[j][i];
						System.out.println("better chessMan found at "
								+ chessBoard.getFields()[j][i]);
					} else if (highest == null
							&& chessBoard.getFields()[j][i].getChessMan()
									.getType() != Type.NONE
							&& chessBoard.getFields()[j][i].getChessMan()
									.getColor() == c) {
						highest = chessBoard.getFields()[j][i];
						System.out.println("good chessMan found at " + highest);
					} else {
						System.out.println(chessBoard.getFields()[j][i]
								.getChessMan()
								+ " at "
								+ chessBoard.getFields()[j][i]
								+ " is not right");
					}
				} else {
					if (chessBoard.getFields()[j + 8][i].getChessMan()
							.getType() == t
							&& chessBoard.getFields()[j + 8][i].getChessMan()
									.getColor() == c) {
						System.out.println("chessMan found at "
								+ chessBoard.getFields()[j + 8][i]);
						return chessBoard.getFields()[j + 8][i];
					} else if (highest != null
							&& highest.getChessMan().getType().getNumber() > chessBoard
									.getFields()[j + 8][i].getChessMan()
									.getType().getNumber()
							&& chessBoard.getFields()[j + 8][i].getChessMan()
									.getType() != Type.NONE
							&& chessBoard.getFields()[j + 8][i].getChessMan()
									.getColor() == c) {
						highest = chessBoard.getFields()[j + 8][i];
						System.out.println("better chessMan found at "
								+ highest);
					} else if (highest == null
							&& chessBoard.getFields()[j + 8][i].getChessMan()
									.getType() != Type.NONE
							&& chessBoard.getFields()[j + 8][i].getChessMan()
									.getColor() == c) {
						highest = chessBoard.getFields()[j + 8][i];
						System.out.println("good chessMan found at " + highest);
					} else {
						System.out.println(chessBoard.getFields()[j + 8][i]
								.getChessMan()
								+ " at "
								+ chessBoard.getFields()[j + 8][i]
								+ " is not right");
					}
				}
			}
		}

		// for (int i = 15; i >= 0; i--) {
		// if (i < 7) {
		// if (chessBoard.getFields()[0][i].getChessMan().getType() != Type.NONE
		// && chessBoard.getFields()[0][i].getChessMan()
		// .getColor() == c) {
		// return chessBoard.getFields()[0][i];
		// }
		// } else {
		// if (chessBoard.getFields()[9][i - 7].getChessMan().getType() !=
		// Type.NONE
		// && chessBoard.getFields()[9][i - 7].getChessMan()
		// .getColor() == c) {
		// return chessBoard.getFields()[9][i - 7];
		// }
		// }
		// }
		return highest;
	}

	private static String getMessage(String command, Matrix m) {
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 4; j++) {
				command += " " + m.get(i, j);
			}
		}
		return command;
	}

	private static void applyMove(Move move) {
		System.out.println(move);
		// TODO zum start bewegen, gripper schließen, zum ziel bewegen, gripper
		// oeffnen
		double grabWidth = 0.001 * ChessMan.GRABWIDTH[move.getStart()
				.getChessMan().getType().getNumber()];
		double grabHeight = ChessMan.GRABHEIGHT[move.getStart().getChessMan()
				.getType().getNumber()];
		// Hoehe setzen

		if (isReachable(getLowMatrix(move.getStart(), grabHeight))) {
			// System.out.println("gW = " + grabWidth);
			// System.out.println("gH = " + grabHeight + "\n");

			// System.out.println("gg1");
			if (robClient.writeAndRead("GripperGoHome") == true)
				;
			// System.out.println("gg1 done");

			Matrix startHigh = getHighMatrix(move.getStart(), grabHeight);
			// System.out.println("sh");
			// System.out.println(startHighMessage);
			if (robClient.writeAndRead(getMessage("MovePTPHomRowWise",
					startHigh)) == true)
				;
			// System.out.println("sh done");

			Matrix start = getLowMatrix(move.getStart(), grabHeight);
			// System.out.println("s");
			// System.out.println(startMessage);
			if (robClient.writeAndRead(getMessage("MovePTPHomRowWise", start)) == true)
				;
			// System.out.println("s done");

			// System.out.println("gmtp");
			if (robClient.writeAndRead("GripperMoveToPosition "
					+ String.valueOf(grabWidth)) == true)
				;
			// System.out.println("gmtp done");

			startHigh = getHighMatrix(move.getStart(), grabHeight);
			// System.out.println("sh");
			if (robClient.writeAndRead(getMessage("MovePTPHomRowWise",
					startHigh)) == true)
				;
			// System.out.println("sh done");

			if (isReachable(getLowMatrix(move.getDestination(), grabHeight))) {

				Matrix destHigh = getHighMatrix(move.getDestination(),
						grabHeight);
				destHigh.print(0, 4);
				// System.out.println("dh");
				if (robClient.writeAndRead(getMessage("MovePTPHomRowWise",
						destHigh)) == true)
					;
				// System.out.println("dh done");

				Matrix dest = getLowMatrix(move.getDestination(), grabHeight);
				dest.print(0, 4);
				// System.out.println("d");
				if (robClient
						.writeAndRead(getMessage("MovePTPHomRowWise", dest)) == true)
					;
				// System.out.println("d done");

				// System.out.println("ggh2");
				if (robClient.writeAndRead("GripperGoHome") == true)
					;
				// System.out.println("gg2 done");

				destHigh = getHighMatrix(move.getDestination(), grabHeight);
				// System.out.println("dh");
				if (robClient.writeAndRead(getMessage("MovePTPHomRowWise",
						destHigh)) == true)
					;
				// System.out.println("dh done");
				// robClient.showHistory();
			} else {
				System.out.println("Dest is not reachable!");
				System.out.println("Enter 'ok' when you are done.");
				waitForInput();
			}
		} else {
			System.out.println("Start is not reachable!");
			System.out.println("Enter 'ok' when you are done.");
			waitForInput();
		}
		move.getDestination().setChessMan(move.getStart().getChessMan());
		move.getStart().setChessMan(new ChessMan(Type.NONE, Color.GRAY));
		chessBoard.output();
		
	}

	private static Matrix getLowMatrix(Field f, double grabHeight) {
		Matrix dest = calculateJointAngles(f);
		if (f.getXPos() != '0' && f.getXPos() != '1') {
			dest = nMatrix.times(dest).times(mMatrix.inverse());
		}
		dest.set(2, 3, dest.get(2, 3) + grabHeight + zOffset);
		dest.setMatrix(0, 2, 0, 2, Matrix.identity(3, 3));
		if (Character.isDigit(f.getXPos())) {
			dest.set(0, 0, 0);
			dest.set(0, 1, 1);
			dest.set(1, 0, -1);
			dest.set(1, 1, 0);
			dest.set(2, 3, dest.get(2, 3) + 1);
		}
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 4; j++) {
				dest.set(i, j, (Math.round(dest.get(i, j) * 100)) / 100d);
			}
		}
		return dest;
	}

	private static Matrix getHighMatrix(Field f, double grabHeight) {
		Matrix dest = getLowMatrix(f, grabHeight);
		Matrix high = new Matrix(4, 4);
		high.set(2, 3, 150);
		if (!Character.isDigit(f.getXPos())) {
			dest.plusEquals(high);
		} else {
			dest.set(2, 3, 500);
		}
		return dest;
	}

	private static void waitForInput() {
		BufferedReader reader = new BufferedReader(new InputStreamReader(
				System.in));
		try {
			String input = reader.readLine();
			while (!input.equals("ok")) {
				input = reader.readLine();
			}
		} catch (IOException e) {
			e.printStackTrace();
		}
	}

	private static Matrix calculateJointAngles(Field f) {
		Matrix homMatrix = null;
		Matrix m;
		double xPos;
		double yPos;
		double zPos;

		char startLetter = f.getXPos();
		int startNumber = f.getYPos();

		if (startLetter == '0') {
			xPos = 200 - (startNumber - 1) * 50;
			yPos = -360;
			zPos = 251;
		} else if (startLetter == '1') {
			xPos = 200 - (startNumber - 1) * 50;
			yPos = -440;
			zPos = 251;
		} else if (startLetter == '2') {
			xPos = 200 - (startNumber - 1) * 50;
			yPos = -520;
			zPos = 251;
		} else if (startLetter == '3') {
			xPos = 200 - (startNumber - 1) * 50;
			yPos = -600;
			zPos = 251;
		} else {
			int count = startLetter - 65;
			xPos = letterStart + (count) * letterLRun + (startNumber - 1)
					* letterNRun;
			yPos = numberStart + (count) * numberLRun + (startNumber - 1)
					* numberNRun;
			zPos = 390 + zOffset + (count) * zLRun + (startNumber - 1) * zNRun;
		}

		homMatrix = Matrix.identity(4, 4);
		// homMatrix = Matrix(4, 4);
		homMatrix.set(0, 3, xPos);
		homMatrix.set(1, 3, yPos);
		homMatrix.set(2, 3, zPos);

		if (!Character.isDigit(startLetter)) {
			m = getBoardPosition().times(homMatrix);
		} else {
			m = homMatrix;
		}
		// m.setMatrix(0, 2, 0, 2, Matrix.identity(3, 3));

		return m;
	}

	private static ArrayList<Move> analyzeMove(String command) {
		ArrayList<Move> moves = new ArrayList<Move>();
		if (command == null) {
			return moves;
		} else if (command.length() < 4) {
			return moves;
			// figuren zurueckstellen
		}
		// System.out.println(Character.toUpperCase(command.charAt(0)));
		Field start = chessBoard.get(command.charAt(0),
				Integer.parseInt(command.substring(1, 2)));
		Field dest = chessBoard.get(command.charAt(2),
				Integer.parseInt(command.substring(3, 4)));
		Move move = new Move(start, dest);
		move.setStartHom(calculateJointAngles(move.getStart()));
		move.setDestHom(calculateJointAngles(move.getDestination()));
		ChessMan chessMan = start.getChessMan();

		System.out.println("startChessMan: " + chessMan);
		System.out.println("destChessMan: " + dest.getChessMan());

		Field enPassantField = null;
		if (chessMan.getColor() == Color.WHITE
				&& move.getDestination().getYPos() > 1) {
			enPassantField = chessBoard.get(dest.getXPos(), dest.getYPos() - 1);
		} else if (chessMan.getColor() == Color.BLACK
				&& move.getDestination().getYPos() < 6) {
			enPassantField = chessBoard.get(dest.getXPos(), dest.getYPos() + 1);
		}
		System.out.println("en passant-Feld:" + enPassantField);
		if (dest.getChessMan().getType() != Type.NONE) {
			// Figur wird geschlagen
			System.out.println("Figur geschlagen");
			Move m = new Move(dest, getNextFreeField());
			m.setStartHom(calculateJointAngles(m.getStart()));
			m.setDestHom(calculateJointAngles(m.getDestination()));
			moves.add(m);
			moves.add(move);

		} else if (enPassantField != null
				&& enPassantField.getChessMan().getType() == Type.PAWN
				&& chessMan.getType() == Type.PAWN
				&& chessMan.getColor() != enPassantField.getChessMan()
						.getColor()) {
			// en-Passant
			System.out.println("en passant");
			Move m = new Move(enPassantField, getNextFreeField()); // Abseits
			m.setStartHom(calculateJointAngles(m.getStart()));
			m.setDestHom(calculateJointAngles(m.getDestination()));
			moves.add(move);
			moves.add(m);

		} else if ((chessMan.getColor() == Color.WHITE
				&& chessMan.getType() == Type.KING
				&& start == chessBoard.get('E', 1) && dest == chessBoard.get(
				'G', 1))
				|| (chessMan.getColor() == Color.BLACK
						&& chessMan.getType() == Type.KING
						&& start == chessBoard.get('E', 8) && dest == chessBoard
						.get('G', 8))) {
			// kleine Rochade
			System.out.println("kleine Rochade");
			Move m1 = new Move(start, chessBoard.get('G', start.getYPos()));
			Move m2 = new Move(chessBoard.get('H', start.getYPos()), chessBoard.get('F', start.getYPos()));
			m1.setStartHom(calculateJointAngles(m1.getStart()));
			m1.setDestHom(calculateJointAngles(m1.getDestination()));
			m2.setStartHom(calculateJointAngles(m2.getStart()));
			m2.setDestHom(calculateJointAngles(m2.getDestination()));
			moves.add(m2);
			moves.add(m1);
		} else if ((chessMan.getColor() == Color.WHITE
				&& chessMan.getType() == Type.KING
				&& start == chessBoard.get('E', 1) && dest == chessBoard.get(
				'C', 1))
				|| (chessMan.getColor() == Color.BLACK
						&& chessMan.getType() == Type.KING
						&& start == chessBoard.get('E', 8) && dest == chessBoard
						.get('C', 8))) {
			// grosse Rochade
			System.out.println("grosse Rochade");
			Move m1 = new Move(start, chessBoard.get('C', start.getYPos()));
			Move m2 = new Move(chessBoard.get('A', start.getYPos()), chessBoard.get('D', start.getYPos()));
			m1.setStartHom(calculateJointAngles(m1.getStart()));
			m1.setDestHom(calculateJointAngles(m1.getDestination()));
			m2.setStartHom(calculateJointAngles(m2.getStart()));
			m2.setDestHom(calculateJointAngles(m2.getDestination()));
			moves.add(m2);
			moves.add(m1);
		} else if (chessMan.getType() == Type.PAWN && (dest.getYPos() == 8
				|| dest.getYPos() == 1)) {
			// Bauerntausch
			System.out.println("Bauerntausch");
			Move m1 = new Move(start, getNextFreeField());
			Type t = Type.NONE;
			if (command.length() > 4) {
				switch (Character.toUpperCase(command.charAt(4))) {
				case 'Q':
					t = Type.QUEEN;
					break;
				case 'R':
					t = Type.ROOK;
					break;
				case 'B':
					t = Type.BISHOP;
					break;
				case 'N':
					t = Type.KNIGHT;
					break;
				default:
					t = Type.NONE;
				}
			}
			System.out.println("Type = " + t);
			Field f = getTakenChessMan(t, start.getChessMan().getColor());
			System.out.println("Field = " + f);
			if (f != null) {
				Move m2 = new Move(f, dest);
				System.out.println("move = " + m2);
				m1.setStartHom(calculateJointAngles(m1.getStart()));
				m1.setDestHom(calculateJointAngles(m1.getDestination()));
				m2.setStartHom(calculateJointAngles(m2.getStart()));
				m2.setDestHom(calculateJointAngles(m2.getDestination()));
				moves.add(m1);
				moves.add(m2);
			} else {
				Move m2 = new Move(start, dest);
				moves.add(m2);
			}

		} else {
			// nichts
			moves.add(move);
		}

		return moves;
	}

	private static boolean calibrate() {
		xMatrices = new ArrayList<Matrix>();
		yMatrices = new ArrayList<Matrix>();

		if (offline) {
			computeMatrices(loadXMatrices(FILE_INDEX),
					loadYMatrices(FILE_INDEX));
		} else if (!offline && !getMatricesOnline()) {
			System.out.println("getMatricesOnline");
			return false;
		}

		Matrix a = new Matrix(120, 12);
		for (int k = 0; k < 10; k++) {
			// System.out.println("y:");
			// yMatrices.get(k).print(0,4);
			for (int i = 0; i < 4; i++) {
				for (int j = 0; j < 4; j++) {
					Matrix m;
					if (i == 3) {
						if (j == 3) {
							m = xMatrices.get(k).getMatrix(0, 2, 0, 2)
									.minus(Matrix.identity(3, 3));
						} else {
							m = new Matrix(new double[][] { { 0, 0, 0 },
									{ 0, 0, 0 }, { 0, 0, 0 } });
						}
					} else if (i == j) {
						m = xMatrices
								.get(k)
								.getMatrix(0, 2, 0, 2)
								.minus(Matrix.identity(3, 3).timesEquals(
										yMatrices.get(k).get(i, i)));
					} else {
						m = Matrix.identity(3, 3)
								.timesEquals(yMatrices.get(k).get(i, j))
								.uminus();
					}
					a.setMatrix(3 * j + k * 12, 3 * j + k * 12 + 2, 3 * i,
							3 * i + 2, m);
				}
			}
		}
		// System.out.println("a:");
		// a.print(0,4);
		Matrix b = new Matrix(120, 1);
		for (int k = 0; k < 10; k++) {
			Matrix m1 = xMatrices.get(k);
			b.set(k * 12 + 9, 0, m1.get(0, 3));
			b.set(k * 12 + 10, 0, m1.get(1, 3));
			b.set(k * 12 + 11, 0, m1.get(2, 3));
		}
		b = b.uminus();
		// System.out.println("b:");
		// b.print(0,4);
		Matrix result = a.solve(b);
		// System.out.println("result:");
		// result.print(0,4);

		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 3; j++) {
				mMatrix.set(j, i, result.get(i * 3 + j, 0));
			}
		}
		mMatrix.set(3, 3, 1);

		Matrix pAT = new Matrix(4, 4);
		Matrix rAE = new Matrix(4, 4);
		if (!offline) {
			trackClientGripper.write("CM_NEXTVALUE");
			while (readMatrix(trackClientGripper) == null) {
				findGripper(new int[] { 0, 0, 0, 0, 0, 0 });
				System.out.println("null");
			}
			// Matrix[] pATs = loadYMatrices(-1);
			// Matrix[] rAEs = loadXMatrices(-1);
			Matrix m;
			for (int i = 0; i < 10; i++) {
				while (getRandomHomMatrix() == null) {
				}
				pAT = getRandomHomMatrix();
				robClient.write("GetPositionHomRowWise");
				rAE = readMatrix(robClient);
				// pAT = pATs[i];
				// rAE = rAEs[i];
				m = rAE.times(mMatrix);
				m = m.times(pAT.inverse());
				nMatrix.plusEquals(m);
			}
			nMatrix.timesEquals(0.1);
		} else {
			Matrix m;
			Matrix[] pATs = loadYMatrices(FILE_INDEX);
			Matrix[] rAEs = loadXMatrices(FILE_INDEX);
			for (int i = 0; i < 10; i++) {
				pAT = pATs[i];
				rAE = rAEs[i];
				m = rAE.times(mMatrix);
				m = m.times(pAT.inverse());
				nMatrix.plusEquals(m);
			}
			nMatrix.timesEquals(0.1);
		}

		// System.out.print("m = ");
		// mMatrix.print(0, 4);
		//
		// System.out.print("n = ");
		// nMatrix.print(0, 4);

		return true;
	}

	private static Matrix getRandomHomMatrix() {
		int[] jointAngles = new int[5];
		jointAngles[0] = 0;
		jointAngles[1] = (int) (Math.random() * 10 - 5);
		jointAngles[2] = (int) (Math.random() * 10 - 5);
		jointAngles[3] = (int) (Math.random() * 90 - 45);
		jointAngles[4] = (int) (Math.random() * 90 - 45);

		if (!findGripper(jointAngles)) {
			System.out.println("not found");
			return null;
		}
		wait(1000);
		return getAverageValue(20);

	}

	public static boolean getMatricesOnline() {
		int attempts = 0;
		Matrix[] xMatricesTemp = new Matrix[10];
		Matrix[] yMatricesTemp = new Matrix[10];
		int count = 0;

		while (count < 10 && attempts < 20) {
			Matrix m1 = getRandomHomMatrix();
			if (m1 != null) {

				robClient.write("GetPositionHomRowWise");
				Matrix m2 = new Matrix(4, 4);
				String[] answer2 = robClient.read().split(" ");

				for (int i = 0; i < 3; i++) {
					for (int j = 0; j < 4; j++) {
						m2.set(i, j, Double.parseDouble(answer2[i * 4 + j]));
					}
				}
				m2.set(3, 3, 1);
				xMatricesTemp[count] = m2;
				yMatricesTemp[count] = m1;
				count++;
			} else {
				System.out.println("not seen");
			}

			attempts++;
		}
		computeMatrices(xMatricesTemp, yMatricesTemp);
		robClient.write("MovePTPJoints 0 0 0 0 0 0");
		robClient.read();

		if (yMatrices.size() < 10) {
			System.out.println("size = " + yMatrices.size());
			return false;
		} else {
			if (save) {
				saveMatrices(xMatricesTemp, yMatricesTemp);
			}
			return true;
		}

	}

	private static Matrix[] loadXMatrices(int index) {
		if (index == -1) {
			index = getFileIndex();
		}
		System.out.println("fileIndex = " + index);
		Matrix[] xMatricesTemp = new Matrix[11];
		try {
			BufferedReader xReader = new BufferedReader(new InputStreamReader(
					new FileInputStream("testValuesArg/xValues" + index
							+ ".txt")));
			for (int k = 0; k < 10; k++) {
				Matrix x = new Matrix(4, 4);
				for (int i = 0; i < 4; i++) {
					String[] xValues = xReader.readLine().split(" ");
					for (int j = 0; j < 4; j++) {
						x.set(i, j, Double.parseDouble(xValues[j]));
					}
				}
				xMatricesTemp[k] = x;
			}
		} catch (FileNotFoundException e) {
			e.printStackTrace();
			return null;
		} catch (IOException e) {
			e.printStackTrace();
			return null;
		}
		return xMatricesTemp;
	}

	private static Matrix[] loadYMatrices(int index) {
		if (index == -1) {
			index = getFileIndex();
		}
		Matrix[] yMatricesTemp = new Matrix[11];
		try {
			BufferedReader yReader = new BufferedReader(new InputStreamReader(
					new FileInputStream("testValuesArg/yValues" + index
							+ ".txt")));
			for (int k = 0; k < 10; k++) {
				Matrix y = new Matrix(4, 4);
				for (int i = 0; i < 4; i++) {
					String[] yValues = yReader.readLine().split(" ");
					for (int j = 0; j < 4; j++) {
						y.set(i, j, Double.parseDouble(yValues[j]));
					}
				}
				yMatricesTemp[k] = y;
			}
		} catch (FileNotFoundException e) {
			e.printStackTrace();
			return null;
		} catch (IOException e) {
			e.printStackTrace();
			return null;
		}
		return yMatricesTemp;
	}

	private static void computeMatrices(Matrix[] xTempOn, Matrix[] yTempOn) {
		for (int i = 0; i < 10; i++) {
			xMatrices.add(xTempOn[(i + 1) % 10].inverse()
					.times(xTempOn[i % 10]));

			// y invers einfuegen?
			yMatrices.add(yTempOn[i % 10].inverse()
					.times(yTempOn[(i + 1) % 10]).inverse());
		}
	}

	private static int getFileIndex() {
		File[] files = (new File("testValuesArg")).listFiles();
		int index = 0;
		for (File f : files) {
			if (f.isFile()) {
				int fileNumber = Integer.parseInt((f.getName().split("\\."))[0]
						.substring(7));
				index = fileNumber > index ? fileNumber : index;
			}
		}
		return index;
	}

	private static void saveMatrices(Matrix[] x, Matrix[] y) {
		int index = getFileIndex() + 1;

		try {
			FileWriter xWriter = new FileWriter("testValuesArg/xValues" + index
					+ ".txt");
			FileWriter yWriter = new FileWriter("testValuesArg/yValues" + index
					+ ".txt");
			for (int k = 0; k < 10; k++) {
				for (int i = 0; i < 4; i++) {
					for (int j = 0; j < 4; j++) {
						xWriter.append(String.valueOf(x[k].get(i, j)) + " ");
						xWriter.flush();
						yWriter.append(String.valueOf(y[k].get(i, j)) + " ");
						yWriter.flush();
						if (j == 3) {
							xWriter.append(System.getProperty("line.separator"));
							xWriter.flush();
							yWriter.append(System.getProperty("line.separator"));
							yWriter.flush();
						}
					}
				}
			}
		} catch (IOException e) {
			e.printStackTrace();
		}
	}

	private static Matrix getAverageValue(int times) {
		Matrix m1 = new Matrix(4, 4);
		for (int i = 0; i < times; i++) {
			trackClientGripper.write("CM_NEXTVALUE");
			Matrix m2 = readMatrix(trackClientGripper);
			if (m2 != null) {
				m1.plusEquals(m2);
			} else {
				i--;
			}
		}
		double d = 1.0 / ((double) times);
		m1.timesEquals(d);
		return m1;
	}

	private static Matrix readMatrix(Client client) {
		String[] s = client.read().split(" ");
		double[][] d = new double[4][4];
		int startIndex = 0;
		if (client == trackClientGripper) {
			startIndex = 2;
		}
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 4; j++) {
				d[i][j] = Double.parseDouble(s[i * 4 + j + startIndex]);
			}
		}
		d[3][3] = 1;
		Matrix m = new Matrix(d);
		return m;
	}

	private static boolean findGripper(int[] angles) {
		String command = "MovePTPJoints ";
		for (int i = 0; i < 5; i++) {
			command += angles[i] + " ";
		}

		for (int i = 0; i < 18; i++) {
			robClient.write(command + String.valueOf(i * (-10)));
			robClient.read();
			trackClientGripper.write("CM_NEXTVALUE");
			String[] answer = trackClientGripper.read().split(" ");
			if (answer[1].equals("y")) {
				return true;
			}
		}
		for (int i = 0; i < 18; i++) {
			robClient.write(command + String.valueOf(i * (10)));
			robClient.read();
			trackClientGripper.write("CM_NEXTVALUE");
			String[] answer = trackClientGripper.read().split(" ");
			if (answer[1].equals("y")) {
				return true;
			}
		}
		return false;
	}

	static private void init() {
		robClient = new RobClient();
		trackClientGripper = new TrackClient();
		trackClientChess = new TrackClient();
		logicClient = new LogicClient();

		if (!offline || connect) {
			if (!robClient.connect(ROB_IP_ADDRESS, ROB_PORT)) {
				System.out.println("cannot connect to roboter");
			} else {
				robClient.write("SetAdeptSpeed 10");
				robClient.read();
				robClient.writeAndRead("MovePTPJoints 0 -30 30 0 0 0");
			}
			System.out.println("1");
			if (!trackClientGripper.connect(TRACK_IP_ADDRESS, TRACK_PORT,
					"Gripper")) {
				System.out.println("cannot connect to tracking system");
			}
			System.out.println("2");
			if (!trackClientChess.connect(TRACK_IP_ADDRESS, TRACK_PORT,
					"ChessBoard")) {
				System.out.println("cannot connect to tracking system");
			}
			System.out.println("3");
			if (connect && !logicClient.connect(LOGIC_IP_ADDRESS, LOGIC_PORT)) {
				System.out.println("cannot connect to logic Server");
			} else if (connect) {
				logicClient.start();
			}
			System.out.println("4");
		} else {
			System.out.println("starting in offline-mode");
			if (connect && !logicClient.connect(LOGIC_IP_ADDRESS, LOGIC_PORT)) {
				System.out.println("cannot connect to logic Server");
			} else if (connect) {
				logicClient.start();
			}
		}

		if (!calibrate()) {
			System.out.println("calibration failed");
		} else {
			robClient.write("SetAllowedStatus noflip up lefty");
			newOffsetter();
		}
	}

}
